//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcRadarTask.h>
#include <rc_mapping/rcmapping.h>
#include <rc_task/rcTaskVariable.h>
#include <rc_log/slog.hpp>


namespace rccore {
    namespace task {

        void
        radar_call_back_function(std::vector<rccore::data_struct::DOT> dots,
                                 std::shared_ptr<common::Context> pcontext) {
//            for (int i = 0; i < dots.size(); i++) {
//                std::cout << "dist: " << dots[i].dist << " theta: " << dots[i].theta << std::endl;
//            }
            pcontext->pmessage_server->pradarMessage->push_message(dots);
        }

        void RadarTask::Run() {
            m_thread = new std::thread([&]() {
                slog::info << "雷达任务启动中->" << pcontext->pconfig->pconfigInfo->LIDAR_DEVICE_PATH << slog::endl;
                mapping::RcMapping rcMapping(pcontext);
                rcMapping.bind(radar_call_back_function);
                rcMapping.start(false, 1);
                task::task_variable::TASK_NUM += 1;

                while (m_isStop) {
                    std::unique_lock<std::mutex> m_locker(m_mutex);
                    m_cv.wait(m_locker, [this] { return !m_isPause; });

                    rcMapping.recive();

                    m_locker.unlock();
                    std::this_thread::sleep_for(std::chrono::microseconds(10));
                }
                rcMapping.stop();
                rcMapping.release();
                slog::info << "雷达任务关闭: " << pcontext->pconfig->pconfigInfo->LIDAR_DEVICE_PATH << slog::endl;
                task::task_variable::TASK_NUM -= 1;
            });
        }

        RadarTask::RadarTask(std::shared_ptr<common::Context> pcontext) : BaseTask(std::move(pcontext)) {

        }
    }
}
